#ifndef WEARHOUSE_NAVIGATOR_HPP
#define WEARHOUSE_NAVIGATOR_HPP

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>

#include "wearhouse_navigation/action/navigate_to_pose.hpp"

#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <std_msgs/msg/string.hpp>

#include "wearhouse_navigation/vel_json.hpp"
#include "wearhouse_navigation/pose_json.hpp"

#include <jsoncpp/json/json.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <Eigen/Dense>
#include <cmath>
#include <algorithm>



namespace wearhouse_navigation {

struct Pose2D 
{
    double x, y, theta;
};

class WearhouseNavigator : public rclcpp::Node {
public:

    WearhouseNavigator();

private:

    // Action Server 相关
    using NavigateToPose = wearhouse_navigation::action::NavigateToPose;
    using GoalHandleNavigateToPose = rclcpp_action::ServerGoalHandle<NavigateToPose>;

    rclcpp_action::Server<NavigateToPose>::SharedPtr action_server_; 

    // Action 回调函数
    rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid,
        std::shared_ptr<const NavigateToPose::Goal> goal);
    rclcpp_action::CancelResponse handle_cancel(
        const std::shared_ptr<GoalHandleNavigateToPose> goal_handle);
    void handle_accepted(const std::shared_ptr<GoalHandleNavigateToPose> goal_handle);

    // 导航执行逻辑
    void execute_navigation(const std::shared_ptr<GoalHandleNavigateToPose> goal_handle);


    // 回调函数
    void poseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);

    // 导航控制函数
    void computeGoalNavigationCommand(double& linear_x, double& linear_y, double& angular_z);
    
    
    // 辅助函数
    double pointToLineDistance(const Eigen::Vector2d& point, const Line& line);
    
    // 速度平滑增加函数
    void applyVelocitySmoothing(double& linear_x, double& linear_y, double& angular_z,
                                            double target_linear_x, double target_linear_y, double target_angular_z);


    // 订阅者和发布者
    rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_;
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr cmd_vel_pub_;
    rclcpp::TimerBase::SharedPtr control_timer_;
    rclcpp::Time last_pose_update_time_;


    // 参数
    int correction_interval_;
    double max_linear_velocity_;
    double max_angular_velocity_;
    double position_tolerance_;
    double orientation_tolerance_;
    // 控制计数器
    int correction_counter_;
    // 速度平滑相关
    double prev_linear_x_ = 0.0;
    double prev_linear_y_ = 0.0;
    double prev_angular_z_ = 0.0;
    double max_acceleration_ = 0.5;  // 最大加速度 (m/s²)
    double max_angular_acceleration_ = 0.5;  // 最大角加速度 (rad/s²)

    // 位姿
    Pose2D current_pose_;
    Pose2D goal_pose_;
    bool pose_valid_;
    bool has_goal_;
    // 状态标志：是否已经完成角度矫正
    bool angle_corrected_ = false;

    
    // Action 执行时的标志
    bool is_navigating_ = false;
    std::shared_ptr<GoalHandleNavigateToPose> active_goal_handle_;

};

} // namespace wearhouse_navigation

#endif // WEARHOUSE_NAVIGATOR_HPP